home *** CD-ROM | disk | FTP | other *** search
/ Language/OS - Multiplatform Resource Library / LANGUAGE OS.iso / cpp_libs / vector.lha / vector / rotmatrix.c < prev    next >
C/C++ Source or Header  |  1991-11-23  |  2KB  |  77 lines
  1. #include "rotmatrix.h"
  2.  
  3. static Vector ZAxis(0,0,1);
  4.  
  5. const int X = 0, Y = 1, Z = 2;
  6.  
  7. // rotate_and_translate creates a transformation matrix
  8. //  encapsulating a rotation followed by a translation.
  9. //
  10. // rot is the rotation vector (in degrees)
  11. // trans is the translation vector
  12. Matrix rotate_and_translate(
  13.     const Vector &rot,
  14.     const Vector &trans) {
  15.  
  16.     Matrix result;
  17.  
  18.     // Z rotation only if rx == ry == 0
  19.     if (rot(X) == 0 && rot(Y) == 0) {
  20.     result = rotate(ZAxis, rot(Z));
  21.     } else {
  22.     // theta is non-Z rotation angle
  23.     float theta = sqrt(rot(X)*rot(X) + rot(Y)*rot(Y));
  24.     Vector xyaxis(rot(X) / theta, rot(Y) / theta, 0);
  25.  
  26.     result = rotate(ZAxis, rot(Z)) * rotate(xyaxis, theta);
  27.     }
  28.  
  29.     // Concatenate desired translation part
  30.     result[X][3]  = trans(X);
  31.     result[Y][3]  = trans(Y);
  32.     result[Z][3]  = trans(Z);
  33.  
  34.     return result;
  35. }
  36.  
  37. // rotate(axis, angle) creates a matrix which rotates by
  38. //  angle degrees around the given rotation axis.
  39. // The algorithm is taken more or less from Newman and Sproull, pp. 346-348.
  40. Matrix rotate(const Vector &a, float angle)
  41. {
  42.     float   v;
  43.     Matrix  R1, R1inv, R2, R2inv, R3;
  44.  
  45.     R1.identity();
  46.     R2.identity();
  47.     R3.identity();
  48.  
  49.     // R1 rotates a around z-axis to x-z plane
  50.     v = sqrt(a(0)*a(0) + a(1)*a(1));
  51.     if (v != 0) {
  52.     R1[0][0] =   R1[1][1] = a(0) / v;
  53.     R1[1][0] = -(R1[0][1] = a(1) / v);
  54.     }
  55.     R1inv = R1;
  56.     R1inv[1][0] = R1[0][1];
  57.     R1inv[0][1] = R1[1][0];
  58.  
  59.     // R2 rotates a around y-axis to x-axis
  60.     R2[0][0] =     R2[2][2] = v;
  61.     R2[2][0] = -(R2[0][2] = a(2));
  62.     R2inv = R2;
  63.     R2inv[2][0] = R2[0][2];
  64.     R2inv[0][2] = R2[2][0];
  65.  
  66.     // R3 rotates around x-axis by angle
  67.     angle = dtor(angle);
  68.     R3[1][1] =     R3[2][2] = cos(angle);
  69.     R3[1][2] = -(R3[2][1] = sin(angle));
  70.  
  71.     // The composition works as follows:
  72.     //    R1 followed by R2 rotates the rotation axis into the X axis
  73.     //    R3 rotates by the desired angle around X
  74.     //    R2inv followed by R1inv rotates back to the rotation axis
  75.     return R1inv * R2inv * R3 * R2 * R1;
  76. }
  77.